#define _USE_MATH_DEFINES // for using some math macros
#include "hdcamerarenderer.h"
#include "hdshareddata.h"

#define SHADER_PATH "E:\\My Desktop\\HD_Chimera\\ChimeraVirtualBox\\HSV.fx"

HDCameraRenderer::HDCameraRenderer(QWidget *parent, int cameraIndex)
	: QGLWidget(parent)
{
	_cameraIndex = cameraIndex;
	setFormat(QGLFormat(QGL::DoubleBuffer | QGL::DepthBuffer | QGL::SampleBuffers ));
	
	desktopWidth = 0;
	desktopHeight = 0;
	markersCount = 0;
	counter = 0;
	texture = 0;
	rubberBandIsShown = false;
	once = true;

	setFocusPolicy(Qt::ClickFocus);
	setMouseTracking(true);
	setAutoFillBackground(false);

	if(!img.load("C:\\Crate.bmp"))
	{
		img.load("\\debug\\Burnout-5-PS3-wallpaper.jpg");
	}

	minHue = 0.0f;
	maxHue = 360.0f;
	minSaturation = 0.0f;
	maxSaturation = 1.0f;
	minValue = 0.0f;
	maxValue = 1.0f;

	_backProjection = false;
	currentObjectIndex = 0;

	desktopWid = QApplication::desktop();
	desktopWidth = desktopWid->screen(desktopWid->primaryScreen())->width();     // returns desktop width
	desktopHeight = desktopWid->screen(desktopWid->primaryScreen())->height();   // returns desktop height

	hdims = 18;
	sdims = 10;
	vdims = 10;

	updateTimer = new QTimer();
	updateTimer->setInterval(1000 / 30);
	connect(updateTimer, SIGNAL(timeout()), this, SLOT(updateGL()));
	updateTimer->start();

	_srcImage = 0;

	markerRadius = 40;
	generateMarkerCursor();

	scaleX = 1;
	scaleY = 1;
}

HDCameraRenderer::~HDCameraRenderer()
{
	glDeleteFramebuffersEXT(1, &fbo);
	glDeleteTextures(1, &texture);
	glDeleteTextures(1, &offscreenTexture2);
	glDeleteTextures(1, &offscreenTexture3);
	glDeleteTextures(1, &offscreenTexture4);
	glDeleteTextures(1, &offscreenTexture5);
	glDeleteTextures(1, &offscreenTexture6);

	cgDestroyEffect(cgEffect);

	// Destroy Our Cg Context And All Programs Contained Within It
	cgDestroyContext(cgContext);
}

/************************************************************************/
/* Initialize the CgFX file, load and compile it.						*/
/************************************************************************/
void HDCameraRenderer::initializeGL()
{
	qglClearColor(Qt::black);
	glShadeModel(GL_FLAT);
	glEnable(GL_DEPTH_TEST);
	//glEnable(GL_CULL_FACE);
	glEnable(GL_TEXTURE_RECTANGLE_ARB);

	initCG();
	initTextures();
}

bool HDCameraRenderer::initCG()
{
	// Setup Cg
	cgContext = cgCreateContext();										// Create A New Context For Our Cg Program(s)

	// Validate Our Context Generation Was Successful
	if (cgContext == NULL)
	{
		QMessageBox::critical(this, "Error", "Failed To Create Cg Context");
		return FALSE;													// We Cannot Continue
	}

	cgGLRegisterStates(cgContext);

	cgEffect = cgCreateEffectFromFile(cgContext, SHADER_PATH, 0);

	if (cgEffect == NULL)
	{
		// We Need To Determine What Went Wrong
		CGerror Error = cgGetError();

		// Show A Message Box Explaining What Went Wrong
		const char* str = cgGetErrorString(Error);
		QMessageBox::critical(this, "Error", str);
		return FALSE;													// We Cannot Continue
	}

	cgTechnique = cgGetFirstTechnique(cgEffect);
	cgValidateTechnique(cgTechnique);

	// Get Handles To Each Of Our Parameters So That
	// We Can Change Them Within Our Code
	objectID = cgGetNamedEffectParameter(cgEffect, "objectID");
	objectsCount = cgGetNamedEffectParameter(cgEffect, "objectsCount");
	centroid = cgGetNamedEffectParameter(cgEffect, "centroid");
	radius = cgGetNamedEffectParameter(cgEffect, "radius");

	origSampler = cgGetNamedEffectParameter(cgEffect, "origSamp");
	modSampler = cgGetNamedEffectParameter(cgEffect, "modifiedSamp");

	hueHistSampler = cgGetNamedEffectParameter(cgEffect, "hueHist");
	satHistSampler = cgGetNamedEffectParameter(cgEffect, "satHist");
	valHistSampler = cgGetNamedEffectParameter(cgEffect, "valHist");

	return TRUE;
}

bool HDCameraRenderer::initTextures()
{
	glGenFramebuffersEXT(1, &fbo);

	glGenTextures( 1, &texture );
	glGenTextures( 1, &textureSmooth );
	glGenTextures( 1, &offscreenTexture2);
	glGenTextures( 1, &offscreenTexture3);
	glGenTextures( 1, &offscreenTexture4);
	glGenTextures( 1, &offscreenTexture5);
	glGenTextures( 1, &offscreenTexture6);

	return TRUE;
}

void HDCameraRenderer::resizeGL(int width, int height)
{
	glViewport(0, 0, width, height);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity( );
	gluOrtho2D(0, width, 0, height);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity( );
	//glTranslatef(0.250, 0.250, 0.0);
	//glTranslatef(0.5, 0.5, 0.0);
}

void HDCameraRenderer::paintGL()
{
	if(captureFinished[_cameraIndex]->tryAcquire())
	{
		bLoading = false;

		// Wait for the Calibration to finish.
		//calibrationFinished->acquire();
		//captureFinished[_cameraIndex]->acquire();

		changeFrame(cameras[_cameraIndex]->frame());
		render();

		// Signal UnprojectionReady to start unprojection.
		unprojectionReady->release();
		//captureReady[_cameraIndex]->release();
	}
	else if(bLoading)
	{
		saveGLState();
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();

		QPainter painter(this);

		QFont font = QApplication::font();
		font.setItalic(true);
		
		painter.setFont(font);
		painter.setPen(QPen(Qt::white, 1));

		painter.drawText(rect(), Qt::AlignCenter, tr("Loading..."));

		restoreGLState();
	}
}
void HDCameraRenderer::render()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);    
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	/************************************************************************/
	/* Start Tracking of the markers.										*/
	/************************************************************************/
	if(objectMarkers.count() > 0)
		trackMarkers();

	/************************************************************************/
	/* Update Histogram.													*/
	/************************************************************************/
	updateHistogram();

	/************************************************************************/
	/* Render the frame to the screen.										*/
	/************************************************************************/
	GLfloat ratio = (float)width() / height();
	GLfloat frameRatio = (float)frameWidth / frameHeight;

	if(ratio >= frameRatio)
		scaleX = frameRatio / ratio;
	else
		scaleY = ratio / frameRatio;

	resizeGL(width(), height());

 	glTranslatef(width() / 2.0f, height() / 2.0f, 0);
	glScalef(scaleX, scaleY, 1);
 	glTranslatef(-width() / 2.0, -height() / 2.0f, 0);

 	if(_backProjection)// && objectMarkers[currentObjectIndex]->pointsCount() > 0)
	{
		renderToScreen(/*offscreenTexture2,//*/objectMarkers[currentObjectIndex]->texOffScreen, 
			QRect(0, frameHeight, frameWidth, -frameHeight), 
 					0, QRect(0, 0, width(), height()));
	}
 	else
	{
		renderToScreen(texture, QRect(0, frameHeight, frameWidth, -frameHeight), 
			0, QRect(0, 0, width(), height()));
	}
	
	/************************************************************************/
	/* Paint 2D																*/
	/************************************************************************/
	//drawForeground();
}


/************************************************************************/
/* Track markers on the GPU.											*/
/************************************************************************/
void HDCameraRenderer::trackMarkers()
{
	/************************************************************************/
	/* ZERO PASS: Smoothing the frame using Box filter.						*/
	/************************************************************************/
	cgGLSetManageTextureParameters(cgContext, CG_TRUE);
	cgGLSetTextureParameter(origSampler, texture);

	cgPass = cgGetFirstPass(cgTechnique);
	renderToTexture(texture, QRect(0, 0, frameWidth, frameHeight), 
		textureSmooth, QRect(0, 0, frameWidth, frameHeight),
		fbo, cgPass);

	for (int i = 0; i < objectMarkers.count(); i++)
	{
		/************************************************************************/
		/* Setup CgFX variables.												*/
		/************************************************************************/
		setupCGParameters(i);

		/************************************************************************/
		/* FIRST PASS: HSV conversion and BackProjection.						*/
		/************************************************************************/
		cgPass = cgGetFirstPass(cgTechnique);
		cgPass = cgGetNextPass(cgPass);
		renderToTexture(textureSmooth, QRect(0, 0, frameWidth, frameHeight), 
			objectMarkers[i]->texOffScreen, QRect(0, 0, frameWidth, frameHeight),
			fbo, cgPass);

		/************************************************************************/
		/* SECOND PASS: Box Filter and Weighted BackProjection.					*/
		/************************************************************************/
		cgPass = cgGetNextPass(cgPass);
		renderToTexture(objectMarkers[i]->texOffScreen, QRect(0, 0, frameWidth, frameHeight),
			offscreenTexture2, QRect(0, 0, frameWidth, frameHeight),
			fbo, cgPass);

		/************************************************************************/
		/* THIRD PASS: Sum of the Intensity, X-CO and Y-CO of the rows(PART1).	*/
		/************************************************************************/
 		cgPass = cgGetNextPass(cgPass);
 		renderToTexture(offscreenTexture2, QRect(0, 0, frameWidth, frameHeight),
 			offscreenTexture3, QRect(0, 0, frameWidth/160, frameHeight), 
 			fbo, cgPass);

		/************************************************************************/
		/* FOURTH PASS: Sum of the Intensity, X-CO and Y-CO of the rows(PART2).	*/
		/************************************************************************/
		cgPass = cgGetNextPass(cgPass);
		renderToTexture(offscreenTexture3, QRect(0, 0, frameWidth/160, frameHeight), 
			offscreenTexture4, QRect(0, 0, 1, frameHeight), 
			fbo, cgPass);

		/************************************************************************/
		/* FIFTH PASS: Sum of the Intensity, X-CO and Y-CO of the columns(PART1)*/
		/************************************************************************/
		cgPass = cgGetNextPass(cgPass);
		renderToTexture(offscreenTexture4, QRect(0, 0, 1, frameHeight), 
			offscreenTexture5, QRect(0, 0, 1, frameHeight/120),
			fbo, cgPass);

		/************************************************************************/
		/* SIXTH PASS: Sum of the Intensity, X-CO and Y-CO of the columns(PART2)*/
		/************************************************************************/
		cgPass = cgGetNextPass(cgPass);
		renderToTexture(offscreenTexture5, QRect(0, 0, 1, frameHeight/120),
			offscreenTexture6, QRect(0, 0, objectMarkers.count(), 1),
			fbo, cgPass);
	}

	/************************************************************************/
	/* Calculate the centroid of the markers.								*/
	/************************************************************************/
	glReadPixels(0, 0, objectMarkers.count(), 1, GL_RGBA, GL_FLOAT, pixels);

	for (int i = 0; i < objectMarkers.count(); i++)
	{
		float cCount = pixels[i * 4 + 0];
		float cX = pixels[i * 4 + 1];
		float cY = pixels[i * 4 + 2];

		cX /= cCount ? cCount : 1;
		cY /= cCount ? cCount : 1;

		if(cCount != 0)
		{
			objectMarkers[i]->setCentroid(QPointF(cX, cY));
			objectMarkers[i]->setPointsCount(cCount);

			searchRadius -= 0.3f;
		}
		else
		{
			searchRadius += 0.3f;
		}

		if(searchRadius > 0.9f)
			searchRadius = 0.9f;
		else if(searchRadius < 0.3)
			searchRadius = 0.3;

// 		QPoint prev(cX * width(), cY * height());
// 		QPoint curr(objectMarkers[i]->centroid().x() * width(), objectMarkers[i]->centroid().y() * height());
// 
// 		QPoint motionVec = prev - curr;
// 		float length = motionVec.manhattanLength();//sqrt(pow((double)motionVec.x(), 2) + pow((double)motionVec.y(), 2));
// 
// 		searchRadius = length / 200.0;
// 		
// 		objectMarkers[i]->setCentroid(QPointF(cX, cY));
// 		objectMarkers[i]->setPointsCount(cCount);

		objectMarkers[i]->setSearchRadius(searchRadius);
	}
}

void HDCameraRenderer::setupCGParameters(int index)
{
	cgGLSetManageTextureParameters(cgContext, CG_TRUE);

	cgGLSetParameter1f(radius, objectMarkers[index]->searchRadius());
	cgGLSetParameter2f(centroid, objectMarkers[index]->centroid().x(), 
		objectMarkers[index]->centroid().y());
	cgGLSetParameter1f(objectID, index);
	cgGLSetParameter1f(objectsCount, markersCount);

	cgGLSetTextureParameter(origSampler, textureSmooth);
	cgGLSetTextureParameter(modSampler, objectMarkers[index]->texOffScreen);

	cgGLSetTextureParameter(hueHistSampler, objectMarkers[index]->texHueHist);
	cgGLSetTextureParameter(satHistSampler, objectMarkers[index]->texSatHist);
	cgGLSetTextureParameter(valHistSampler, objectMarkers[index]->texValHist);
}

void HDCameraRenderer::renderToTexture(GLuint sourceTexture, QRect srcRect,
	GLuint distTexture, QRect distRect, GLuint frameBufferObject, CGpass pass)
{
	glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, frameBufferObject);

	if(frameBufferObject)
	{
		glFramebufferTexture2DEXT(GL_FRAMEBUFFER_EXT, 
			GL_COLOR_ATTACHMENT0_EXT, GL_TEXTURE_RECTANGLE_ARB, distTexture, 0);
	}

	glPushAttrib(GL_VIEWPORT_BIT);
	resizeGL(distRect.width(), distRect.height());

	cgSetPassState(pass);

	if(sourceTexture)
	{
		glBindTexture(GL_TEXTURE_RECTANGLE_ARB, sourceTexture);
		glColor3f(1.0, 1.0, 1.0);
	}
	else
		glColor3f(0.0, 0.0, 0.0);

	glBegin(GL_QUADS);
	glTexCoord2i(srcRect.left()	, srcRect.top());			glVertex2i(distRect.left(), distRect.top());
	glTexCoord2i(srcRect.right()+1, srcRect.top());			glVertex2i(distRect.right()+1, distRect.top());
	glTexCoord2i(srcRect.right()+1, srcRect.bottom()+1);	glVertex2i(distRect.right()+1, distRect.bottom()+1);
	glTexCoord2i(srcRect.left()	, srcRect.bottom()+1);		glVertex2i(distRect.left(), distRect.bottom()+1);
	glEnd();

	cgResetPassState(pass);

	glPopAttrib();
}

void HDCameraRenderer::renderToScreen(GLuint sourceTexture, QRect srcRect,
									   GLuint distTexture, QRect distRect)
{
	glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);

	if(sourceTexture)
	{
		glBindTexture(GL_TEXTURE_RECTANGLE_ARB, sourceTexture);
		glColor3f(1.0, 1.0, 1.0);
	}
	else
		glColor3f(0.0, 0.0, 0.0);

	glBegin(GL_QUADS);
	glTexCoord2i(srcRect.left()	, srcRect.top());			glVertex2i(distRect.left(), distRect.top());
	glTexCoord2i(srcRect.right()+1, srcRect.top());			glVertex2i(distRect.right()+1, distRect.top());
	glTexCoord2i(srcRect.right()+1, srcRect.bottom()+1);	glVertex2i(distRect.right()+1, distRect.bottom()+1);
	glTexCoord2i(srcRect.left()	, srcRect.bottom()+1);		glVertex2i(distRect.left(), distRect.bottom()+1);
	glEnd();
}

/************************************************************************/
/* Draw Foreground.														*/
/************************************************************************/
void HDCameraRenderer::drawForeground()
{
	saveGLState();
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	QPainter painter(this);

	/************************************************************************/
	/* Draw the objects cursor.												*/
	/************************************************************************/
	drawObjectsCursor(&painter);

	/************************************************************************/
	/* Draw the text.														*/
	/************************************************************************/
	drawText(&painter);

	startDemo(&painter);

	/************************************************************************/
	/* Draw the selection.													*/
	/************************************************************************/
	if (rubberBandIsShown) 
	{
		painter.setCompositionMode(QPainter::CompositionMode_Exclusion);

		painter.setPen(QPen(Qt::white, 1, Qt::DashLine, Qt::SquareCap));
		painter.setBrush(QBrush(QColor(92,128,255,20), Qt::SolidPattern));
		painter.drawRect(QRect(pntA, mapFromGlobal(QCursor::pos())).normalized().adjusted(0, 0, -1, -1));

		painter.setCompositionMode(QPainter::CompositionMode_SourceOver);
	}

	// 	if (markersCount > 0 && objectMarkers[currentObjectIndex]->bAdaptiveVal) 
	// 	{
	// 		painter.setCompositionMode(QPainter::CompositionMode_Exclusion);
	// 
	// 		painter.setPen(QPen(Qt::white, 1, Qt::DashLine, Qt::SquareCap));
	// 		painter.setBrush(QBrush(QColor(92,128,255,20), Qt::SolidPattern));
	// 		painter.drawRect(QRect(sX * 2, sY * 2, sWidth * 2, sHeight * 2));
	// 
	// 		painter.setCompositionMode(QPainter::CompositionMode_SourceOver);
	// 	}

	if(_cameraIndex == currentCameraIndex)
		painter.setPen(QPen(Qt::yellow, 3));
	else
		painter.setPen(QPen(Qt::black, 3));

	painter.setBrush(Qt::NoBrush);
	painter.drawRect(rect().adjusted(0, 0, -1, -1));
	painter.end();

	restoreGLState();
}

void HDCameraRenderer::saveGLState()
{
	glPushAttrib(GL_ALL_ATTRIB_BITS);

	glMatrixMode(GL_PROJECTION);
	glPushMatrix();

	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
}

void HDCameraRenderer::drawObjectsCursor( QPainter * painter )
{
	painter->setBrush(Qt::NoBrush);

	for (int i=0; i< objectMarkers.count(); i++)
	{
		if(objectMarkers[i]->pointsCount() > 0)
		{
			float cX = objectMarkers[i]->centroid().x() * width() * scaleX;
			float cY = objectMarkers[i]->centroid().y() * height() * scaleY;

			cX += (width() / 2.0 - width() * scaleX / 2.0);
			cY += (height() / 2.0 - height() * scaleY / 2.0);

			//painter->setCompositionMode(QPainter::CompositionMode_Exclusion);

			painter->drawPixmap(cX - pmMarkerCursor.width() / 2, cY - pmMarkerCursor.height() / 2, pmMarkerCursor);

			//painter->setCompositionMode(QPainter::CompositionMode_SourceOver);

			QFont f("Arial", 28, QFont::Normal);
			painter->setPen(QPen(Qt::white, 4));
			painter->setBrush(Qt::NoBrush);
			painter->setFont(f);
			painter->drawText(QRect(cX - markerRadius/2, cY - markerRadius / 2, markerRadius, markerRadius),
				Qt::AlignCenter, tr("%0").arg(i+1));
		}
	}
}

void HDCameraRenderer::drawText( QPainter * painter )
{
	painter->setFont( QApplication::font());
	painter->setPen(QPen(Qt::white, 4));

	// FPS Calculations.
	if((++counter %= 10) == 0)
		timeElapsed = fpsTime.restart();

	painter->drawText(10, 20, tr("FPS = %0").arg(timeElapsed ? 10000 / timeElapsed : 0));

	if(markersCount > 0)
	{
		painter->drawText(10, 35, tr("Count = %0").arg(objectMarkers[currentObjectIndex]->pointsCount()));
		painter->drawText(10, 50, tr("X-Co = %0").arg(objectMarkers[currentObjectIndex]->centroid().x()));
		painter->drawText(10, 65, tr("Y-Co = %0").arg(objectMarkers[currentObjectIndex]->centroid().y()));

		painter->drawText(10, 85, tr("searchRadius = %0").arg(searchRadius));
	}
}
/************************************************************************/
/* Demos Handling.														*/
/************************************************************************/
void HDCameraRenderer::startDemo( QPainter *painter )
{
	switch (_demoID)
	{
	case DEMO_NONE:
		break;

	case DEMO_MOUSE:
		startMouseDemo();
		break;

	case DEMO_IMAGE:
		startImage2DDemo(painter);
		break;

	case DEMO_IMAGE3:
		startImage3DDemo(painter);
		break;
	}
}

void HDCameraRenderer::startMouseDemo()
{
	float borders = 0.04f;

	float cX = objectMarkers[currentObjectIndex]->centroid().x();
	float cY = objectMarkers[currentObjectIndex]->centroid().y();

	QCursor::setPos(QPoint(desktopWidth * (cX/*floor(cX * frameWidth) / frameWidth*/ - borders) / (1 - 2 * borders),
		desktopHeight * (cY/*floor(cY * frameHeight) / frameHeight*/ - borders) / (1 - 2 * borders)));
}

void HDCameraRenderer::startImage2DDemo( QPainter *painter )
{
	if(markersCount >= 2)
	{	
		QPoint bottomRight(objectMarkers[_cameraIndex + 0 * camerasCount]->centroid().x() * frameWidth, 
			objectMarkers[_cameraIndex + 0 * camerasCount]->centroid().y() * frameHeight);

		QPoint upperLeft(objectMarkers[_cameraIndex + 1 * camerasCount]->centroid().x() * frameWidth, 
			objectMarkers[_cameraIndex + 1 * camerasCount]->centroid().y() * frameHeight);

		int dx = bottomRight.x() - upperLeft.x();
		int dy = bottomRight.y() - upperLeft.y();
		QPoint pnt;

		float scaledDiagonal = (float)sqrt((float)(dx * dx + dy * dy));
		float _imageScale = scaledDiagonal / (float)sqrt((float)(img.width() * img.width() + img.height() * img.height()));

		float w = width();
		float h = height();
		float tx = 0, ty = 0;

		float ratio = w / h;
		float frameRatio = (float)frameWidth / frameHeight;
		float scaleX, scaleY, s;

		if(ratio >= frameRatio)
		{
			tx = (w - h*4.0f/3.0f)/2.0f;
			s = h / frameHeight;
		}
		else
		{
			ty = (h - w*3.0f/4.0f)/2.0f;
			s = w / frameWidth;
		}

		pnt.setX(upperLeft.x() + img.width() * _imageScale);
		pnt.setY(upperLeft.y() + img.height() * _imageScale);

		glTranslatef(s*upperLeft.x() + tx, s*upperLeft.y() + ty, 0);
		glRotatef((atan2((float)dy, (float)dx) - atan2((float)img.height(), (float)img.width())) * 180.0f / 3.14f ,0,0,1);
		glTranslatef(-s*upperLeft.x() - tx, -s*upperLeft.y() - ty, 0);

		painter->drawImage(QRectF(s * upperLeft.x() + (tx), s* upperLeft.y() + (ty), 
			s * (pnt.x() - upperLeft.x()), s* (pnt.y() - upperLeft.y())), img);

	}
}

void HDCameraRenderer::startImage3DDemo( QPainter *painter )
{
	if(markersCount >= 4)
	{
		QPoint upperLeft(objectMarkers[0]->centroid().x() * frameWidth, 
			objectMarkers[0]->centroid().y() * frameHeight);

		QPoint upperRight(objectMarkers[1]->centroid().x() * frameWidth, 
			objectMarkers[1]->centroid().y() * frameHeight);

		QPoint bottomRight(objectMarkers[2]->centroid().x() * frameWidth,
			objectMarkers[2]->centroid().y() * frameHeight);

		QPoint bottomLeft(objectMarkers[3]->centroid().x() * frameWidth,
			objectMarkers[3]->centroid().y() * frameHeight);


		int dx = upperLeft.x() - bottomLeft.x();
		int dy = upperLeft.y() - bottomLeft.y();

		int left = (float)sqrt((float)(dx * dx + dy * dy));

		dx = upperRight.x() - bottomRight.x();
		dy = upperRight.y() - bottomRight.y();

		int right = (float)sqrt((float)(dx * dx + dy * dy));

		float rotationY = 180.0f * ((right / (float)left) - 0.8f) / 0.4f - 90.0f;

		glTranslatef(upperLeft.x() * 2, 0, 0);
		glRotatef(rotationY, 0, 1, 0);
		glTranslatef(-upperLeft.x() * 2, 0, 0);

		painter->drawImage(QRectF(upperLeft.x() * 2, upperLeft.y() * 2, 
			left * 2, left * 2), img);

		painter->drawText(upperLeft.x() * 2, upperLeft.y() * 2, tr("RotationY = %0").arg(rotationY));
	}
}


/************************************************************************/
void HDCameraRenderer::restoreGLState()
{
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();

	glMatrixMode(GL_PROJECTION);
	glPopMatrix();

	glPopAttrib();
}
/************************************************************************/
void HDCameraRenderer::setFrame(uchar* data, int width, int height)
{
	//QMutexLocker locker(&mutex);

	frameWidth = width;
	frameHeight = height;

	makeCurrent();

	if(once)
	{
		glBindTexture(GL_TEXTURE_RECTANGLE_ARB, textureSmooth);
		glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA32F_ARB,  frameWidth, frameHeight, 0, GL_RGBA, GL_FLOAT, NULL);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST );

		/************************************************************************/
		/* SECOND PASS:															*/
		/************************************************************************/
		glBindTexture(GL_TEXTURE_RECTANGLE_ARB, offscreenTexture2);
		glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA32F_ARB,  frameWidth, frameHeight, 0, GL_RGBA, GL_FLOAT, NULL);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
// 		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB,GL_TEXTURE_WRAP_S,GL_CLAMP);
// 		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB,GL_TEXTURE_WRAP_T,GL_CLAMP);

		/************************************************************************/
		/* THIRD PASS:															*/
		/************************************************************************/
		glBindTexture(GL_TEXTURE_RECTANGLE_ARB, offscreenTexture3);
		glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA32F_ARB , frameWidth / 160, frameHeight, 0, GL_RGBA, GL_FLOAT, NULL);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
// 		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB,GL_TEXTURE_WRAP_S,GL_CLAMP);
// 		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB,GL_TEXTURE_WRAP_T,GL_CLAMP);

		/************************************************************************/
		/* FOURTH PASS:													*/
		/************************************************************************/
		glBindTexture(GL_TEXTURE_RECTANGLE_ARB, offscreenTexture4);
		glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA32F_ARB, 1, frameHeight, 0, GL_RGBA, GL_FLOAT, NULL);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
// 		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB,GL_TEXTURE_WRAP_S,GL_CLAMP);
// 		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB,GL_TEXTURE_WRAP_T,GL_CLAMP);
		
		/************************************************************************/
		/* FIFTH PASS:															*/
		/************************************************************************/
		glBindTexture(GL_TEXTURE_RECTANGLE_ARB, offscreenTexture5);
		glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA32F_ARB, 1, frameHeight / 120, 0, GL_RGBA, GL_FLOAT, NULL);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
// 		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB,GL_TEXTURE_WRAP_S,GL_CLAMP);
// 		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB,GL_TEXTURE_WRAP_T,GL_CLAMP);

		once = false;
	}

	//if(objectMarkers.count() != markersCount)
	{
		markersCount = objectMarkers.count();
		camerasCount = cameras.count();
		/************************************************************************/
		/* FIRST PASS:															*/
		/************************************************************************/
		for (int i=0; i < objectMarkers.count(); i++)
		{

			if(!objectMarkers[i]->texOffScreen)
			{
				glGenTextures( 1, &(objectMarkers[i]->texOffScreen));

				glGenTextures( 1, &(objectMarkers[i]->texHueHist));
				glGenTextures( 1, &(objectMarkers[i]->texSatHist));
				glGenTextures( 1, &(objectMarkers[i]->texValHist));
			}

			glBindTexture(GL_TEXTURE_RECTANGLE_ARB, objectMarkers[i]->texOffScreen);
			glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA32F_ARB,  frameWidth, frameHeight, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
			glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
			glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
// 			glTexParameteri(GL_TEXTURE_RECTANGLE_ARB,GL_TEXTURE_WRAP_S,GL_CLAMP);
// 			glTexParameteri(GL_TEXTURE_RECTANGLE_ARB,GL_TEXTURE_WRAP_T,GL_CLAMP);
		}

		/************************************************************************/
		/* SIXTH PASS :															*/
		/************************************************************************/
		glBindTexture(GL_TEXTURE_RECTANGLE_ARB, offscreenTexture6);
		glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA32F_ARB, objectMarkers.count(), 1, 0, GL_RGBA, GL_FLOAT, NULL);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
// 		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_S,GL_CLAMP);
// 		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_T,GL_CLAMP);
	}

	/************************************************************************/
	/* Remove Noise using OpenCV:											*/
	/************************************************************************/
	if (!_srcImage)
		_srcImage = cvCreateImage( cvSize(frameWidth, frameHeight), 8, 3 );

	_srcImage->imageData = (char*)data;
	//cvSmooth(_srcImage, _srcImage, CV_BLUR, 9);
	//cvSmooth(_srcImage, _srcImage, CV_MEDIAN, 1);

	/************************************************************************/
	/* FINAL SURFACE RENDERER:												*/
	/************************************************************************/
	glBindTexture(GL_TEXTURE_RECTANGLE_ARB, texture );
	glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA, frameWidth, frameHeight, 
		0, GL_BGR_EXT, GL_UNSIGNED_BYTE, _srcImage->imageData );
	glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
	glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
// 	glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_S,GL_CLAMP);
// 	glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_T,GL_CLAMP);

	//img = QImage((uchar*)_srcImage->imageData, frameWidth, frameHeight, QImage::Format_RGB888);
	//img = img.rgbSwapped();

	//updateGL();
}

void HDCameraRenderer::changeFrame(IplImage *frame)
{
	setFrame((uchar*)(frame->imageData), frame->width, frame->height);
}
/************************************************************************/
QSize HDCameraRenderer::sizeHint()
{
	return QSize(320, 240);
}

/************************************************************************/
/* Keyboard Handlers.													*/
/************************************************************************/
void HDCameraRenderer::keyPressEvent(QKeyEvent *event)
{
	switch(event->key())
	{
	case Qt::Key_A:
		markerRadius += 5;
		generateMarkerCursor();
		break;

	case Qt::Key_Z:
		markerRadius -= 5;
		generateMarkerCursor();
		break;
	}
}
void HDCameraRenderer::keyReleaseEvent(QKeyEvent * event)
{

}
/************************************************************************/
/* Mouse Handlers.														*/
/************************************************************************/
void HDCameraRenderer::mousePressEvent(QMouseEvent *event)
{
	if(currentCameraIndex != _cameraIndex)
	{
		currentCameraIndex = _cameraIndex;
		
		if (objectMarkers.count() > 0)
			updateHistogramPanels();

		emit focusChanged(_cameraIndex);
	}
	
	if(event->buttons() & Qt::LeftButton)
	{
		setCursor(Qt::CrossCursor);
		bMouseDown = true;

// 		if (objectMarkers.count() > 0)
// 		{
// 			objectMarkers[currentObjectIndex]->bAdaptiveHue = true;
// 			objectMarkers[currentObjectIndex]->bAdaptiveSat = true;
// 			objectMarkers[currentObjectIndex]->bAdaptiveVal = true;
// 
// 			objectMarkers[currentObjectIndex]->bCalcHueHist = false;
// 			objectMarkers[currentObjectIndex]->bCalcSatHist = false;
// 			objectMarkers[currentObjectIndex]->bCalcValHist = false;
// 		}

		pntA = pntB = event->pos();
	}
}

void HDCameraRenderer::mouseMoveEvent(QMouseEvent *event)
{
	if(currentCameraIndex != _cameraIndex)
		unsetCursor();

	if(bMouseDown && (Qt::LeftButton == event->buttons() & Qt::LeftButton))
	{
		rubberBandIsShown = true;
		pntB = event->pos();

		rctSelection = QRect(pntA, pntB).normalized().adjusted(0, 0, -1, -1);

		float scaleX, scaleY;
		float w = width();
		float h = height();
		float ratio = w / h;
		float frameRatio = (float)frameWidth / frameHeight;

		if(ratio >= frameRatio)
		{
			rctSelection.translate(-(w - h*4.0f/3.0f)/2.0, 0);

			scaleX = GLfloat(frameWidth) / (h * 4.0f / 3.0f);
			scaleY = GLfloat(frameHeight) / GLfloat(h);
		}
		else
		{
			rctSelection.translate(0 , -(h - w*3.0f/4.0f)/2.0f);

			scaleX = GLfloat(frameWidth) / GLfloat(w);
			scaleY = GLfloat(frameHeight) / (w * 3.0f / 4.0f);
		}

		rctSelection.setWidth(rctSelection.width() * scaleX);
		rctSelection.setHeight(rctSelection.height() * scaleY);
		rctSelection.moveTo(rctSelection.x() * scaleX, rctSelection.y() * scaleY);
		rctSelection = rctSelection.normalized();

		if (objectMarkers.count() > 0)
		{
			objectMarkers[currentObjectIndex]->bAdaptiveHue = true;
			objectMarkers[currentObjectIndex]->bAdaptiveSat = true;
			objectMarkers[currentObjectIndex]->bAdaptiveVal = true;

			objectMarkers[currentObjectIndex]->bCalcHueHist = !objectMarkers[currentObjectIndex]->rangeHue();
			objectMarkers[currentObjectIndex]->bCalcSatHist = !objectMarkers[currentObjectIndex]->rangeSat();
			objectMarkers[currentObjectIndex]->bCalcValHist = !objectMarkers[currentObjectIndex]->rangeVal();
		}
	}
}

void HDCameraRenderer::mouseReleaseEvent(QMouseEvent* event)
{
	if(bMouseDown)
	{
		bMouseDown = false;
	}

	if(rubberBandIsShown)//&& (Qt::LeftButton == event->buttons() & Qt::LeftButton))
	{
		rubberBandIsShown = false;
		//unsetCursor();

		if(rctSelection.width() == 0 || rctSelection.height() == 0 || objectMarkers.count() == 0)
			return;

		if (objectMarkers.count() > 0)
		{
			objectMarkers[currentObjectIndex]->bAdaptiveHue = false;
			objectMarkers[currentObjectIndex]->bAdaptiveSat = false;
			objectMarkers[currentObjectIndex]->bAdaptiveVal = false;

			objectMarkers[currentObjectIndex]->bCalcHueHist = !objectMarkers[currentObjectIndex]->rangeHue();
			objectMarkers[currentObjectIndex]->bCalcSatHist = !objectMarkers[currentObjectIndex]->rangeSat();
			objectMarkers[currentObjectIndex]->bCalcValHist = !objectMarkers[currentObjectIndex]->rangeVal();
		}

		updateGL();
	}
}
/************************************************************************/
/* Update Histogram.													*/
/************************************************************************/
void HDCameraRenderer::updateHistogram()
{
	int sX = 0, sY = 0, sWidth = 0, sHeight = 0;

	for(int i = 0; i < objectMarkers.count(); i++ )
	{
		if(objectMarkers[i]->bCalcHueHist || 
			objectMarkers[i]->bCalcSatHist || 
			objectMarkers[i]->bCalcValHist)
		{
// 			if((objectMarkers[i]->bAdaptiveHue || 
//				objectMarkers[i]->bAdaptiveSat || 
//				objectMarkers[i]->bAdaptiveVal) 
// 				&& objectMarkers[i]->pointsCount() > 0)
// 			{
// 				rctSelection.moveTo(objectMarkers[i]->centroid().x() * frameWidth - rctSelection.width() / 2, 
// 					objectMarkers[i]->centroid().y() * frameHeight - rctSelection.height() / 2);
// 			}

			sX = rctSelection.x();
			sY = rctSelection.y();
			sWidth = rctSelection.width();
			sHeight = rctSelection.height();

			if(sX < 0)
			{
				sWidth = sWidth + sX;
				sX = 0;
			}
			else if(sX >= frameWidth)
				sX = frameWidth - 1;

			if(sY < 0)
			{
				sHeight = sHeight + sY;
				sY = 0;
			}
			else if(sY >= frameHeight)
				sY = frameHeight - 1;

			if(sX + sWidth >= frameWidth)
				sWidth = frameWidth - 1 - sX;

			if(sY + sHeight >= frameHeight)
				sHeight = frameHeight - 1 - sY;


			if(!objectMarkers[i]->rangeHue() ||
				!objectMarkers[i]->rangeSat() ||
				!objectMarkers[i]->rangeVal())
			{
				if(sWidth == 0 || sHeight == 0)
					continue;

				glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
				glBindTexture(GL_TEXTURE_RECTANGLE_ARB, texture);
				glGetTexImage(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA, GL_FLOAT, pixels);
			}

			/************************************************************************/
			/* Calculate Hue Histogram.												*/
			/************************************************************************/
			if(objectMarkers[i]->bCalcHueHist)
			{
				if(!objectMarkers[i]->bAdaptiveHue)
					objectMarkers[i]->bCalcHueHist = false;

				memset(objectMarkers[i]->hBins, 0, sizeof(objectMarkers[i]->hBins));

				if(!objectMarkers[i]->rangeHue())
				{
					float maxColor, minColor;
					float r, g, b, hue, temp;

					for (int k = sY; k < sHeight + sY; k++)
					{
						for (int j = sX; j < sWidth + sX; j++)
						{
							r = pixels[4 * (k * (frameWidth) + j) + 0];
							g = pixels[4 * (k * (frameWidth) + j) + 1];
							b = pixels[4 * (k * (frameWidth) + j) + 2];

							maxColor = max(r, g);
							maxColor = max(b, maxColor);

							minColor = min(r, g);
							minColor = min(b, minColor);

							if(maxColor == minColor)
							{	
								hue = 0;
							}
							else if( maxColor == r)
							{
								temp = 60.0f * (g - b) / (maxColor - minColor) + 360.0f;
								hue = temp - 360 * floor(temp / 360.0f);
							}
							else if( maxColor == g)
							{
								hue = 60.0f * (b - r) / (maxColor - minColor) + 120.0f;
							}
							else if( maxColor == b)
							{
								hue = 60.0f * (r - g) / (maxColor - minColor) + 240.0f;
							}

							int index = ceil(hue * hdims / 360.0f );

							if (index != 0)
								index--;

							objectMarkers[i]->hBins[index]++;
						}
					}

					float maxHistBin = 0;

					for (int j = 0; j < hdims; j++)
					{
						if(objectMarkers[i]->hBins[j] > maxHistBin)
							maxHistBin = objectMarkers[i]->hBins[j];
					}

					if (maxHistBin == 0)
						break;

					for (int j = 0; j < hdims; j++)
					{
						objectMarkers[i]->hBins[j] /= maxHistBin;
					}
				}
				else
				{
					minHue = objectMarkers[i]->minHue();
					maxHue = objectMarkers[i]->maxHue();

					if(minHue <= maxHue)
					{
						for (int j = 0; j < hdims; j++)
						{
							objectMarkers[i]->hBins[j] = gaussian(1, (minHue + maxHue) / 2.0f, abs(maxHue - minHue) / 4.0f, 
								360.0f * j / (float)hdims);
						}
					}
					else
					{
						for (int j = 0; j < hdims; j++)
						{
							objectMarkers[i]->hBins[j] = gaussian(1, (minHue + maxHue + 360.0) / 2.0f, abs(maxHue + 360.0 - minHue) / 4.0f, 
								360.0f * j / (float)hdims);
						}

						for (int j = 0; j < hdims; j++)
						{
							float temp = gaussian(1, (minHue - 360.0 + maxHue) / 2.0f, abs(maxHue - (minHue - 360.0)) / 4.0f, 
								360.0f * j / (float)hdims);

							if(temp > objectMarkers[i]->hBins[j])
								objectMarkers[i]->hBins[j] = temp;
						}
					}
				}

				glBindTexture(GL_TEXTURE_RECTANGLE_ARB, objectMarkers[i]->texHueHist);
				glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_LUMINANCE, hdims, 1, 0, GL_LUMINANCE, GL_FLOAT, 
					objectMarkers[i]->hBins);
				glTexParameteri( GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
				glTexParameteri( GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
			}

			/************************************************************************/
			/* Calculate Saturation Histogram.										*/
			/************************************************************************/
			if(objectMarkers[i]->bCalcSatHist)
			{
				if(!objectMarkers[i]->bAdaptiveSat)
					objectMarkers[i]->bCalcSatHist = false;

				memset(objectMarkers[i]->sBins, 0, 
					sizeof(objectMarkers[i]->sBins));

				if(!objectMarkers[i]->rangeSat())
				{
					float maxColor, minColor;
					float r, g, b, sat;

					for (int k = sY; k < sHeight + sY; k++)
					{
						for (int j = sX; j < sWidth + sX; j++)
						{
							r = pixels[4 * (k * (frameWidth) + j) + 0];
							g = pixels[4 * (k * (frameWidth) + j) + 1];
							b = pixels[4 * (k * (frameWidth) + j) + 2];

							maxColor = max(r, g);
							maxColor = max(b, maxColor);

							minColor = min(r, g);
							minColor = min(b, minColor);

							if(maxColor == 0)
								sat = 0;
							else
								sat = 1.0f - minColor / maxColor;

							int index = ceil(sat * sdims );
							
							if (index != 0)
								index--;

							objectMarkers[i]->sBins[index]++;
						}
					}

					float maxHistBin = 0;

					for (int j = 0; j < sdims; j++)
					{
						if(objectMarkers[i]->sBins[j] > maxHistBin)
							maxHistBin = objectMarkers[i]->sBins[j];
					}
					
					for (int j = 0; j < sdims; j++)
					{
						objectMarkers[i]->sBins[j] /= maxHistBin;
					}
				}
				else
				{
					minSaturation = objectMarkers[i]->minSaturation();
					maxSaturation = objectMarkers[i]->maxSaturation();

					if(minSaturation <= maxSaturation)
					{
						for (int j = 0; j < sdims; j++)
						{
							objectMarkers[i]->sBins[j] = gaussian(1, (minSaturation + maxSaturation) / 2.0f, 
								abs(maxSaturation - minSaturation) / 4.0f, j / (float)sdims);
						}
					}
					else
					{
						for (int j = 0; j < sdims; j++)
						{
							objectMarkers[i]->sBins[i] = gaussian(1, (minSaturation + maxSaturation + 1.0) / 2.0f, 
								abs(maxSaturation + 1.0 - minSaturation) / 4.0f, j / (float)sdims);
						}

						for (int j = 0; j < sdims; j++)
						{
							float temp = gaussian(1, (minSaturation - 1.0 + maxSaturation) / 2.0f, 
								abs(maxSaturation - (minSaturation - 1.0)) / 4.0f, j / (float)sdims);

							if(temp > objectMarkers[i]->sBins[j])
								objectMarkers[i]->sBins[j] = temp;
						}
					}
				}

				glBindTexture(GL_TEXTURE_RECTANGLE_ARB, objectMarkers[i]->texSatHist);
				glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_LUMINANCE, sdims, 1, 0, GL_LUMINANCE, GL_FLOAT, 
					objectMarkers[i]->sBins);
				glTexParameteri( GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
				glTexParameteri( GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
			}

			/************************************************************************/
			/* Calculate Value Histogram.											*/
			/************************************************************************/
			if(objectMarkers[i]->bCalcValHist)
			{
				if(!objectMarkers[i]->bAdaptiveVal)
					objectMarkers[i]->bCalcValHist = false;

				memset(objectMarkers[i]->vBins, 0, 
					sizeof(objectMarkers[i]->vBins));

				if(!objectMarkers[i]->rangeVal())
				{
					float maxColor;
					float r, g, b, val;

					for (int k = sY; k < sHeight + sY; k++)
					{
						for (int j = sX; j < sWidth + sX; j++)
						{
							r = pixels[4 * (k * (frameWidth) + j) + 0];
							g = pixels[4 * (k * (frameWidth) + j) + 1];
							b = pixels[4 * (k * (frameWidth) + j) + 2];

							maxColor = max(r, g);
							val = max(b, maxColor);

							int index = ceil(val * vdims );

							if (index != 0)
								index--;

							objectMarkers[i]->vBins[index]++;
						}
					}

					float maxHistBin = 0;

					for (int j = 0; j < vdims; j++)
					{
						if(objectMarkers[i]->vBins[j] > maxHistBin)
							maxHistBin = objectMarkers[i]->vBins[j];
					}

					for (int j = 0; j < vdims; j++)
					{
						objectMarkers[i]->vBins[j] /= maxHistBin;
					}
				}
				else
				{
					minValue = objectMarkers[i]->minValue();
					maxValue = objectMarkers[i]->maxValue();

					if(minValue <= maxValue)
					{
						for (int j = 0; j < vdims; j++)
						{
							objectMarkers[i]->vBins[j] = gaussian(1, (minValue + maxValue) / 2.0f, 
								abs(maxValue- minValue) / 4.0f, j / (float)vdims);
						}
					}
					else
					{
						for (int j = 0; j < vdims; j++)
						{
							objectMarkers[i]->vBins[j] = gaussian(1, (minValue + maxValue + 1.0) / 2.0f, 
								abs(maxValue + 1.0 - minValue) / 4.0f, j / (float)vdims);
						}

						for (int j = 0; j < vdims; j++)
						{
							float temp = gaussian(1, (minValue - 1.0 + maxValue) / 2.0f, 
								abs(maxValue - (minValue - 1.0)) / 4.0f, j / (float)vdims);

							if(temp > objectMarkers[i]->vBins[j])
								objectMarkers[i]->vBins[j] = temp;
						}
					}
				}

				glBindTexture(GL_TEXTURE_RECTANGLE_ARB, objectMarkers[i]->texValHist);
				glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_LUMINANCE, vdims, 1, 0, GL_LUMINANCE, GL_FLOAT, 
					objectMarkers[i]->vBins);
				glTexParameteri( GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
				glTexParameteri( GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
			}

			updateHistogramPanels();
		}
	}
}

float HDCameraRenderer::gaussian(float amplitude, float mean, 
								 float sigma, float x)
{
	float y = 0;

	if(sigma != 0)
		y = amplitude * exp(-((x - mean)*(x - mean)) / (2 * sigma * sigma));

	return y;
}
void HDCameraRenderer::updateHistogramPanels()
{
	return;

	/************************************************************************/
	/* Display the Hue Histogram Windows.									*/
	/************************************************************************/
	IplImage *histimgH = cvCreateImage( cvSize(320,200), 8, 3 );
	cvZero( histimgH );

	int bin_w = histimgH->width / hdims;

	for( int i = 0; i < hdims; i++ )
	{
		int val = cvRound(objectMarkers[currentObjectIndex]->hBins[i] * histimgH->height);
		QColor binColor = QColor::fromHsv(360 * i/hdims, 255, 255);
		CvScalar color = cvScalar(binColor.blue(), binColor.green(), binColor.red());

		cvRectangle( histimgH, cvPoint(i*bin_w,histimgH->height),
			cvPoint((i+1)*bin_w,histimgH->height - val),
			color, -1, 8, 0 );
	}

	cvNamedWindow( "HueHistogram", 1 );
	cvShowImage( "HueHistogram", histimgH );

	cvReleaseImage(&histimgH);


	/************************************************************************/
	/* Display the Sat Histogram Windows.									*/
	/************************************************************************/
	IplImage *histimgS = cvCreateImage( cvSize(320,200), 8, 3 );
	cvZero( histimgS );

	bin_w = histimgS->width / sdims;
	for( int i = 0; i < sdims; i++ )
	{
		int val = cvRound(objectMarkers[currentObjectIndex]->sBins[i] * histimgS->height);
		CvScalar color = cvScalar(i * 205.f / sdims + 50, i * 205.f/sdims + 50, i*205.f/sdims + 50);

		cvRectangle( histimgS, cvPoint(i * bin_w, histimgS->height),
			cvPoint((i+1) * bin_w, histimgS->height - val),
			color, -1, 8, 0 );
	}

	cvNamedWindow( "SaturationHistogram", 1 );
	cvShowImage( "SaturationHistogram", histimgS );

	cvReleaseImage(&histimgS);


	/************************************************************************/
	/* Display the Val Histogram Windows.									*/
	/************************************************************************/
	IplImage *histimgV = cvCreateImage( cvSize(320,200), 8, 3 );
	cvZero( histimgV );

	bin_w = histimgV->width / vdims;

	for( int i = 0; i < vdims; i++ )
	{
		int val = cvRound( objectMarkers[currentObjectIndex]->vBins[i] * histimgV->height);
		CvScalar color = cvScalar(205.0 * i/vdims + 50, i*205.f/vdims + 50, i*205.f/vdims + 50);
		cvRectangle( histimgV, cvPoint(i*bin_w,histimgV->height),
			cvPoint((i+1) * bin_w, histimgV->height - val), 
			color, -1, 8, 0 );
	}

	cvNamedWindow( "ValueHistogram", 1 );
	cvShowImage( "ValueHistogram", histimgV );

	cvReleaseImage(&histimgV);
}
/************************************************************************/
/*                                                                      */
/************************************************************************/
void HDCameraRenderer::addMarker()
{
	objectMarkers.append(new HDObject(this));
}

HDObject* HDCameraRenderer::selectedMarker()
{
	return objectMarkers[currentObjectIndex];
}

bool HDCameraRenderer::backProjection()
{
	return _backProjection;
}

void HDCameraRenderer::setBackProjection(bool enable)
{
	_backProjection = enable;
}

HDDemoID HDCameraRenderer::demoID()
{
	return _demoID;
}

void HDCameraRenderer::setDemoID(HDDemoID id)
{
	_demoID = id;
}
void HDCameraRenderer::generateMarkerCursor()
{
	pmMarkerCursor = QPixmap(5 * markerRadius / 4, 5 * markerRadius / 4);
	pmMarkerCursor.fill(Qt::transparent);

	int center = 5 * markerRadius / 8;

	QPainter painter(&pmMarkerCursor);
	
	painter.setBrush(Qt::NoBrush);

	painter.setPen(QPen(Qt::yellow, 2));

	painter.drawEllipse(center - markerRadius / 2.0f, center - markerRadius / 2.0f, markerRadius, markerRadius);
	painter.drawEllipse(center - markerRadius / 3, center - markerRadius / 3, markerRadius * 2 / 3, markerRadius * 2 / 3);
	painter.drawEllipse(center - markerRadius / 8, center - markerRadius / 8, markerRadius / 4, markerRadius / 4);

	painter.drawLine(QPoint(0, center), QPoint(center - markerRadius / 8, center));
	painter.drawLine(QPoint(center + markerRadius / 8, center), QPoint(pmMarkerCursor.width(), center));

	painter.drawLine(QPoint(center, 0), QPoint(center, center - markerRadius / 8));
	painter.drawLine(QPoint(center, center + markerRadius / 8), QPoint(center, pmMarkerCursor.width()));
}